#include "collision_detection.h"
#include "log/zuclog.h"
#include "dyna/jaka_dyn_intf.h"
#include <math.h>

#define COLL_INFO(fmt, ...) zuclog_info("COLL", fmt, ##__VA_ARGS__)
#define COLL_ERR(fmt, ...) zuclog_error("COLL", fmt, ##__VA_ARGS__)
#define COLL_DEBUG(fmt, ...) zuclog_debug("COLL", fmt, ##__VA_ARGS__)

using namespace axisgroup;

CollisionDetection::CollisionDetection(std::shared_ptr<JAKADynIntf> dyna) : dyn_(dyna) {}

CollisionDetection::SaftyMonitorRes CollisionDetection::SaftyAction(const Robot::MovingState& state) { return SaftyMonitorRes::NO_ALARM; }

int CollisionDetection::collisionDetectionOnMomentumObserver(const Robot::MovingState& state, const double* toolParameters)
{
    // #TODO 获得的机器人运动速度由于通讯问题，很可能存在错误的速度，即速度突然到零，此时会干扰观测器的运行，需要考虑消除影响
    if (has_wrong_vel)
    {
        return 0;
    }

    // 2. 计算动态阈值
    RobJointData<double> joint_sensitivity;
    GMGetCollisionSensitivity(
        state.cmd.cmd_pos, state.fdb.fdb_vel, state.cmd.cmd_vel, state.cmd.cmd_acc, state.cmd.cmd_jerk, toolParameters, joint_sensitivity);

    // 3. 计算观测值
    RobJointData<double> torpEst;
    RobJointData<double> torpEst_F;   // 低通滤波
    RobJointData<double> torpEst_N;   // 陷波滤波
    RobJointData<double> torpEst_N1;  // 陷波滤波

    dyn_->collisionDetect(state.fdb.fdb_pos.val, state.fdb.fdb_vel.val, state.fdb.fdb_torque.val, torpEst_N.val);

    RobJointData<double> motor_trans;  // #TODO 需要知道减速比
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        torpEst_N1[i] = notch_filter1_[i].do_filter(state.cmd.cmd_vel.val[i], torpEst_N[i], 125, motor_trans[i], 0.001);
    }
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        torpEst_F[i] = notch_filter1_[2].do_filter(state.cmd.cmd_vel.val[i] / 2.0, torpEst_N1[i], 125, motor_trans[i], 0.001);
    }
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++) { torpEst[i] = low_pass_filter_[i].do_filter(30, torpEst_F[i]); }

    int collision_flag = 0;
    for (int i = RobJointData<double>::MAX_AXIS; i >= 0; i--)
    {
        if (fabs(torpEst[i]) > joint_sensitivity[i])
        {
            collision_flag = i + 1;
            break;
        }
    }

    if (collision_flag)
    {
        collision_count++;
    }
    else
    {
        collision_count = 0;
    }

    do_record(state, joint_sensitivity, torpEst, torpEst_N, torpEst_N1, torpEst_F, collision_count, collision_flag);
    return collision_count >= MAX_DETECT_COUNT ? collision_flag : 0;
}

void CollisionDetection::do_record(const Robot::MovingState& state,
                                   const RobJointData<double>& joint_sensitivity,
                                   const RobJointData<double>& torpEst,
                                   const RobJointData<double>& torpEst_N,
                                   const RobJointData<double>& torpEst_N1,
                                   const RobJointData<double>& torpEst_F,
                                   int collision_count,
                                   int collision_flag)
{
    if (_gm_record_enable && _gm_debug_file)
    {
        fprintf(_gm_debug_file, "%s", state.cmd.cmd_pos.val_str().c_str());
        fprintf(_gm_debug_file, "%s", state.fdb.fdb_pos.val_str().c_str());
        fprintf(_gm_debug_file, "%s", state.fdb.fdb_vel.val_str().c_str());
        fprintf(_gm_debug_file, "%s", state.fdb.fdb_torque.val_str().c_str());
        fprintf(_gm_debug_file, "%s", state.cmd.cmd_torque.val_str().c_str());
        fprintf(_gm_debug_file, "%s", torpEst.val_str().c_str());
        fprintf(_gm_debug_file, "%s", joint_sensitivity.val_str().c_str());
        fprintf(_gm_debug_file, "%s", torpEst_N.val_str().c_str());
        fprintf(_gm_debug_file, "%s", torpEst_N1.val_str().c_str());
        fprintf(_gm_debug_file, "%s", torpEst_F.val_str().c_str());
        fprintf(_gm_debug_file, "%d, ", collision_count);
        fprintf(_gm_debug_file, "%d, ", collision_flag);
        fprintf(_gm_debug_file, "\n");
    }
    if (collision_count >= MAX_DETECT_COUNT)
    {
        history_gm_record_count = 1;
    }
    GMHistoryRecord(state, torpEst, joint_sensitivity, collision_count, collision_flag);
}

void CollisionDetection::set_end_force_limit(double limit)
{
    COLL_INFO("End Force Limit change from %lf to %lf", end_force_limit_, limit);
    end_force_limit_ = limit;
}

void CollisionDetection::set_joint_sensitivity(const RobJointData<double>& joint_sensitivity)
{
    // 隐藏界面的关节灵敏度设置界面
    // #TODO 关联到伺服参数的设置
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        if (joint_sensitivity.val[i] > 90)
        {
            // 现场经常遇到机器人不关碰撞没法跑的情况，这里留个后门，大于90阈值直接放大两倍
            joint_sensitivity_[i] = joint_sensitivity.val[i] * 2;
        }
    }
}

void CollisionDetection::set_GM_factor(const double* factor)
{
    for (int i = 0; i < 7; i++) { GM_factor_[i] = factor[i]; }
}

void CollisionDetection::GMGetCollisionSensitivity(const RobJointData<double>& jntPosCmd,
                                                   const RobJointData<double>& jntVelFb,
                                                   const RobJointData<double>& jntVelCmd,
                                                   const RobJointData<double>& jntAccCmd,
                                                   const RobJointData<double>& jntJerkCmd,
                                                   const double* toolParameters,
                                                   RobJointData<double>& joint_sensitivity)
{
    // 根据用户设定level和机器人型号，获取阈值基值

    RobJointData<double> high_speed_compensate_factor;
    RobJointData<double> low_speed_compensate_factor;
    RobJointData<double> high_jerk_compensate_factor;
    RobJointData<double> high_acc_compensate_factor;
    RobJointData<double> tool_compensate_fator;

    GMGetBaseSensitivityFromApp(toolParameters[0],
                                high_jerk_compensate_factor,
                                joint_sensitivity,
                                high_speed_compensate_factor,
                                low_speed_compensate_factor,
                                high_acc_compensate_factor,
                                tool_compensate_fator);

    // 高速摩擦力模型误差补偿
    RobJointData<double> high_comp;
    RobJointData<double> HIGH_VEL_COMPENSATE_ACTIVE_VALYE_1;  // 补偿生效速度
    RobJointData<double> HIGH_VEL_COMPENSATE_ACTIVE_VALYE_2;  // 补偿生效速度
    double high_vel_compensate_factor = 0.5;                  // 越小上升越快
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        if (fabs(jntVelFb.val[i]) > HIGH_VEL_COMPENSATE_ACTIVE_VALYE_1[i])
        {
            high_comp[i] += 1.0 / sqrt(1.0 + pow(high_vel_compensate_factor / (fabs(jntVelFb.val[i]) - HIGH_VEL_COMPENSATE_ACTIVE_VALYE_1[i] + 0.001), 2)) *
                            high_speed_compensate_factor[i];
        }
        if (fabs(jntVelFb.val[i]) > HIGH_VEL_COMPENSATE_ACTIVE_VALYE_2[i])
        {
            high_comp[i] += 1.0 / sqrt(1.0 + pow(high_vel_compensate_factor / (fabs(jntVelFb.val[i]) - HIGH_VEL_COMPENSATE_ACTIVE_VALYE_2[i] + 0.001), 2)) *
                            high_speed_compensate_factor[i];
        }
    }

    // 低速摩擦力模型误差补偿
    RobJointData<double> low_comp;
    const double low_vel_compensate_factor = 200.0;  // 越大下降越快
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        if (fabs(jntVelFb.val[i]) < 0.01 || fabs(jntVelCmd.val[i]) < 0.005)
        {
            low_comp[i] = low_speed_compensate_factor[i] * 1.5;
            continue;
        }
        low_comp[i] += (1.0 / (fabs(jntVelCmd.val[i]) * low_vel_compensate_factor + 1)) * low_speed_compensate_factor[i];
        low_comp[i] += (1.0 / (fabs(jntVelCmd.val[i]) * low_vel_compensate_factor / 5.0 + 1)) * low_speed_compensate_factor[i] / 5.0;
    }

    // 高加加速度补偿
    RobJointData<double> jerk_comp;
    RobJointData<double> joint_jerk_compensate;
    GMGetJointJerkCompensate(jntJerkCmd, toolParameters, high_jerk_compensate_factor, joint_jerk_compensate);

    RobJointData<double> load_comp;
    RobJointData<double> high_acc_comp;
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++) { jerk_comp[i] += joint_jerk_compensate[i]; }

    double max_acc[7] = {400, 400, 400, 400, 400, 400, 400};  // #TODO 读取机器人最大关节加速度
    // 高加速度补偿，重载机器人电机能力不幸，高加速度带不起来
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        if (fabs(jntAccCmd.val[i]) > max_acc[i] / 2.0)
        {
            high_acc_comp[i] = 1.0 / sqrt(1.0 + pow(10.0 / (fabs(jntAccCmd.val[i]) - max_acc[i] / 2.0 + 0.001), 2)) * high_acc_compensate_factor[i];
        }
    }

    for (int i = 0; i < 3; i++) { load_comp[i] += toolParameters[0] * tool_compensate_fator[i]; }
    for (size_t i = 3; i < RobJointData<double>::MAX_AXIS; i++) { load_comp[i] += toolParameters[0] * tool_compensate_fator[i]; }

    // #TODO 根据臂展进行调整
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        joint_sensitivity[i] += high_comp[i] + low_comp[i] + jerk_comp[i] + load_comp[i] + high_acc_comp[i];
    }

    // 下降滤波器
    double filt_t = 5.0 * 0.008;
    double k1 = 0.008 / (2.0 * filt_t + 0.008);
    double k2 = -(0.008 - 2.0 * filt_t) / (0.008 + 2.0 * filt_t);
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        joint_sensitivity_filted[i] = k2 * joint_sensitivity_filted[i] + k1 * joint_sensitivity[i] + k1 * last_joint_sensitivity[i];
        last_joint_sensitivity[i] = joint_sensitivity[i];
        joint_sensitivity[i] = joint_sensitivity_filted[i] > joint_sensitivity[i] ? joint_sensitivity_filted[i] : joint_sensitivity[i];
    }
}

void CollisionDetection::GMGetBaseSensitivityFromApp(double tool_mass,
                                                     RobJointData<double>& high_jerk_compensate_factor,
                                                     RobJointData<double>& joint_basic_sensitivity,
                                                     RobJointData<double>& high_speed_compensate_factor,
                                                     RobJointData<double>& low_speed_compensate_factor,
                                                     RobJointData<double>& high_acc_compensate_factor,
                                                     RobJointData<double>& tool_compensate_fator)
{
    int collision_sensity_level = 5;  // get_robot_joint_collision_sensitivity();  // #TODO 读取碰撞灵敏度等级
    if (collision_sensity_level < 0 || collision_sensity_level > 7)
    {
        COLL_ERR("collision_sensity_level error, %d", collision_sensity_level);
        collision_sensity_level = 0;
    }
    if (collision_sensity_level < last_collision_sensity_level)
    {
        // 由于切碰撞等级时，伺服可能存在抖动，这时候观测器容易产生误报，因此延迟切换碰撞等级
        count_change_level++;
        if (count_change_level > LEVEL_DOWN_DELAY)
        {
            last_collision_sensity_level = collision_sensity_level;
            count_change_level = 0;
        }
    }
    else
    {
        last_collision_sensity_level = collision_sensity_level;
    }

    double sensetivity_factor = GM_factor_[last_collision_sensity_level];

#if 0
    RobJointData<double> motor_rated_tor;  // #TODO 读取各个关节的额定力矩
    int robot_model = 0;          // 读取机型
    switch (robot_model)
    {
    case MODEL_ZU_310:
    case MODEL_ZU_311:
    case MODEL_ZU_312:
    case MODEL_ZU_313: {
        for (int i = 0; i < 3; i++)
        {
            joint_basic_sensitivity[i] = zucmotStatus->servo_collision_base[i] / 1.5 +
                                         zucmotStatus->servo_collision_level[i] / 2.0 * (25 + zucmotConfig->safetyCfg.joint_collision_sensitivity[i]) / 100.0;
        }
        for (int i = 3; i < RobJointData<double>::MAX_AXIS; i++)
        {
            joint_basic_sensitivity[i] = zucmotStatus->servo_collision_base[i] / 1.5 +
                                         zucmotStatus->servo_collision_level[i] / 2.0 * (25 + zucmotConfig->safetyCfg.joint_collision_sensitivity[i]) / 100.0;
        }

        high_speed_compensate_factor[0] = motor_rated_tor[0] * 0.05;
        high_speed_compensate_factor[1] = motor_rated_tor[1] * 0.05;
        high_speed_compensate_factor[2] = motor_rated_tor[2] * 0.05;
        high_speed_compensate_factor[3] = 39 * 0.05;
        high_speed_compensate_factor[4] = 39 * 0.05;
        high_speed_compensate_factor[5] = 39 * 0.05;

        low_speed_compensate_factor[0] = motor_rated_tor[0] * 0.05;
        low_speed_compensate_factor[1] = motor_rated_tor[1] * 0.05;
        low_speed_compensate_factor[2] = motor_rated_tor[2] * 0.05;
        low_speed_compensate_factor[3] = 39 * 0.05;
        low_speed_compensate_factor[4] = 39 * 0.05;
        low_speed_compensate_factor[5] = 39 * 0.05;

        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            high_jerk_compensate_factor[i] = zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i] * collision_sensity_level;
            high_acc_compensate_factor[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (tool_mass + 0.6) / 6.0;
        }

        // 2023.05.28 make tool compensate larger
        tool_compensate_fator[0] = 0.5;
        tool_compensate_fator[1] = 0.5;
        tool_compensate_fator[2] = 0.5;
        tool_compensate_fator[3] = 0.3;
        tool_compensate_fator[4] = 0.3;
        tool_compensate_fator[5] = 0.3;
        break;
    }
    case MODEL_ZU_1210:
    case MODEL_ZU_1211:
    case MODEL_ZU_1212:
    case MODEL_PRO_1213: {
        for (int i = 0; i < 3; i++)
        {
            joint_basic_sensitivity[i] =
                (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (force_limit / 100.0 + 0.7 + (-40 + joint_limit[i]) / 100.0);
        }
        for (int i = 3; i < RobJointData<double>::MAX_AXIS; i++)
        {
            joint_basic_sensitivity[i] = (zucmotStatus->servo_collision_base[i] / 3.0 + zucmotStatus->servo_collision_level[i] / 2.0) *
                                         (force_limit / 100.0 + 0.7 + (-40 + joint_limit[i]) / 100.0);
        }

        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            high_speed_compensate_factor[i] = motor_rated_tor[i] * 0.05;
            low_speed_compensate_factor[i] = motor_rated_tor[i] * 0.03;
            high_jerk_compensate_factor[i] = zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i] * collision_sensity_level;
            high_acc_compensate_factor[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (tool_mass + 2.4) / 24.0;
        }

        tool_compensate_fator[0] = 0.5;
        tool_compensate_fator[1] = 0.5;
        tool_compensate_fator[2] = 0.5;
        tool_compensate_fator[3] = 0.1;
        tool_compensate_fator[4] = 0.1;
        tool_compensate_fator[5] = 0.1;
        break;
    }
    case MODEL_ZU_1810:
    case MODEL_ZU_1811:
    case MODEL_ZU_1812:
    case MODEL_PRO_1813:
    case MODEL_ZU_2010:
    case MODEL_ZU_2011:
    case MODEL_ZU_2012:
    case MODEL_PRO_1610:
    case MODEL_ZU_2020: {
        for (int i = 0; i < 3; i++)
        {
            joint_basic_sensitivity[i] =
                (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (force_limit / 100.0 + 0.7 + (-40 + joint_limit[i]) / 100.0);
        }
        for (int i = 3; i < RobJointData<double>::MAX_AXIS; i++)
        {
            joint_basic_sensitivity[i] = (zucmotStatus->servo_collision_base[i] / 3.0 + zucmotStatus->servo_collision_level[i] / 2.0) *
                                         (force_limit / 100.0 + 0.7 + (-40 + joint_limit[i]) / 100.0);
        }

        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            high_speed_compensate_factor[i] = motor_rated_tor[i] * 0.05;
            low_speed_compensate_factor[i] = motor_rated_tor[i] * 0.03;
            high_jerk_compensate_factor[i] = zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i] * collision_sensity_level;
            high_acc_compensate_factor[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (tool_mass + 3.6) / 36.0;
        }

        tool_compensate_fator[0] = 1.0;
        tool_compensate_fator[1] = 1.0;
        tool_compensate_fator[2] = 1.0;
        tool_compensate_fator[3] = 0.6;
        tool_compensate_fator[4] = 0.6;
        tool_compensate_fator[5] = 0.6;
        break;
    }
    case MODEL_CO_0115:
    case MODEL_CO_1115:
    case MODEL_CO_1315:
    case MODEL_CO_1415:
    case MODEL_CO_2015: {
        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            joint_basic_sensitivity[i] = zucmotStatus->servo_collision_base[i] +
                                         zucmotStatus->servo_collision_level[i] * (25 + zucmotConfig->safetyCfg.joint_collision_sensitivity[i]) / 100.0;
        }

        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            high_speed_compensate_factor[i] = motor_rated_tor[i] * 0.05;
            low_speed_compensate_factor[i] = motor_rated_tor[i] * 0.03;
            high_jerk_compensate_factor[i] = zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i] * collision_sensity_level;
            high_acc_compensate_factor[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (tool_mass + 0.4) / 4.0;
        }

        tool_compensate_fator[0] = 1.0;
        tool_compensate_fator[1] = 1.0;
        tool_compensate_fator[2] = 1.0;
        tool_compensate_fator[3] = 0.3;
        tool_compensate_fator[4] = 0.3;
        tool_compensate_fator[5] = 0.3;
        break;
    }
    case MODEL_ZU_710:
    case MODEL_ZU_720:
    case MODEL_ZU_721:
    case MODEL_ZU_722:
    case MODEL_ZU_723:
    case MODEL_ZU_724:
    case MODEL_ZU_725:
    case MODEL_ZU_728:
    case MODEL_PRO_731:
    case MODEL_ZU_512:
    case MODEL_ZU_513:
    case MODEL_ZU_514:
    case MODEL_PRO_515:
    case MODEL_ZU_726:
    case MODEL_ZU_727:
    case MODEL_C_510:
    case MODEL_C_511:
    case MODEL_C_710:
    case MODEL_C_711: {
        for (int i = 0; i < 3; i++)
        {
            joint_basic_sensitivity[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) *
                                         (end_force_limit_ / 100.0 + 0.7 + (-40 + zucmotConfig->safetyCfg.joint_collision_sensitivity[i]) / 100.0);
        }
        for (int i = 3; i < RobJointData<double>::MAX_AXIS; i++)
        {
            joint_basic_sensitivity[i] = (zucmotStatus->servo_collision_base[i] / 3.0 + zucmotStatus->servo_collision_level[i] / 2.0) *
                                         (end_force_limit_ / 100.0 + 0.7 + (-40 + zucmotConfig->safetyCfg.joint_collision_sensitivity[i]) / 100.0);
        }
        // joint_basic_sensitivity[0] = 18 + 8 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;
        // joint_basic_sensitivity[1] = 10 + 8 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;
        // joint_basic_sensitivity[2] = 10 + 8 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;
        // joint_basic_sensitivity[3] = 5 + 5 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;
        // joint_basic_sensitivity[4] = 5 + 5 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;
        // joint_basic_sensitivity[5] = 5 + 5 * (zucmotConfig->safetyCfg.joint_collision_sensitivity[i] + 25) / 100.0;

        for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
        {
            high_speed_compensate_factor[i] = motor_rated_tor[i] * 0.03;
            low_speed_compensate_factor[i] = motor_rated_tor[i] * 0.02;
            high_jerk_compensate_factor[i] = zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i] * collision_sensity_level;
            high_acc_compensate_factor[i] = (zucmotStatus->servo_collision_base[i] + zucmotStatus->servo_collision_level[i]) * (tool_mass + 1.4) / 14.0;
        }

        // 2023.05.28 make tool compensate larger
        tool_compensate_fator[0] = 0.3;
        tool_compensate_fator[1] = 0.3;
        tool_compensate_fator[2] = 0.3;
        tool_compensate_fator[3] = 0.1;
        tool_compensate_fator[4] = 0.1;
        tool_compensate_fator[5] = 0.1;
        break;
    }
    }
#endif
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++) { joint_basic_sensitivity[i] *= sensetivity_factor; }
}

void CollisionDetection::GMGetJointJerkCompensate(const RobJointData<double>& joint_jerk,
                                                  const double* toolParameters,
                                                  const RobJointData<double>& high_jerk_compensate_factor,
                                                  RobJointData<double>& joint_jerk_compensate)
{
    // accumulate jerk value for 10  period
    RobJointData<double> jerk_accumlate_value;
    for (size_t i = 0; i < RobJointData<double>::MAX_AXIS; i++)
    {
        jerk_acuumulator_[i].push_back(joint_jerk.val[i]);
        if (jerk_acuumulator_->size() > MAX_ACCU_NUM)
        {
            jerk_acuumulator_[i].pop_front();
        }
        for (auto jerk_value : jerk_acuumulator_[i]) { jerk_accumlate_value[i] += jerk_value; }
    }
    for (size_t j = 0; j < RobJointData<double>::MAX_AXIS; j++)
    {
        if (j < 3)
        {
            joint_jerk_compensate[j] =
                (high_jerk_compensate_factor.val[j] + toolParameters[0] * 2.0) * exp(-1.0 / (pow(fabs(jerk_accumlate_value[j]), 3) / 25000000.0 + 0.01));
        }
        else
        {
            joint_jerk_compensate[j] =
                (high_jerk_compensate_factor.val[j] + toolParameters[0] * 2) * exp(-1.0 / (pow(fabs(jerk_accumlate_value[j]), 3) / 10000000.0 + 0.01));
            joint_jerk_compensate[j] +=
                (high_jerk_compensate_factor.val[j] + toolParameters[0] * 1.5) / 5 * exp(-1.0 / (pow(fabs(jerk_accumlate_value[j]), 3) / 1000000.0 + 0.01));
        }
    }
}
void CollisionDetection::enableGmRecord(int enable)
{
    if (enable && !_gm_record_enable)
    {
        if (_gm_debug_file)
        {
            fclose(_gm_debug_file);
            _gm_debug_file = NULL;
        }
        _gm_debug_file = fopen("/tmp/GM_log.txt", "w");  // #TODO 多机器人冲突
    }
    if (!enable && _gm_record_enable)
    {
        if (_gm_debug_file)
        {
            fclose(_gm_debug_file);
            _gm_debug_file = NULL;
        }
    }
    _gm_record_enable = enable;
}

void CollisionDetection::GMHistoryRecord(const Robot::MovingState& state,
                                         const RobJointData<double>& torpEst,
                                         const RobJointData<double>& joint_sensitivity,
                                         int collision_count,
                                         int collision_flag)
{
    if (hist_data_.size() > HISTORY_RECORD)
    {
        hist_data_.pop_front();
    }
    HishtoryData cur_data;
    cur_data.history_collision_count_value = collision_count;
    cur_data.history_collision_flag_value = collision_flag;
    cur_data.history_linear_vel_value = state.cmd.linear_cmd_vel;
    cur_data.history_torEst_value = torpEst;
    cur_data.history_sensitivity_value = joint_sensitivity;
    cur_data.history_qd_value = state.fdb.fdb_vel;
    cur_data.history_q_cmd_value = state.cmd.cmd_pos;
    cur_data.history_q_value = state.fdb.fdb_pos;
    cur_data.history_jointTorqFb_value = state.fdb.fdb_torque;
    cur_data.history_jointTorqCmd_value = state.cmd.cmd_torque;
    hist_data_.push_back(cur_data);

    if (history_gm_record_count > 0 && history_gm_record_count < 2 * HISTORY_RECORD && gm_hist_record_enable)
    {
        if (history_gm_record_count == 1 && !gm_hist_record_file)
        {
            time_t timep;
            struct tm* p;
            time(&timep);
            p = gmtime(&timep);
            char file_name[40];
            sprintf(file_name, "/tmp/%d%d%d-%d:%d-GMDebug.txt", (1900 + p->tm_year), (1 + p->tm_mon), p->tm_mday, 8 + p->tm_hour, p->tm_min);
            gm_hist_record_file = fopen(file_name, "a");
        }
        if (gm_hist_record_file)
        {
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_q_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_q_cmd_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_qd_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_jointTorqFb_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_jointTorqCmd_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_torEst_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%s", hist_data_.front().history_sensitivity_value.val_str().c_str());
            fprintf(gm_hist_record_file, "%lf", hist_data_.front().history_linear_vel_value);
            fprintf(gm_hist_record_file, "%d, ", hist_data_.front().history_collision_flag_value);
            fprintf(gm_hist_record_file, "%d, ", hist_data_.front().history_collision_count_value);
            fprintf(gm_hist_record_file, "\n");
        }
        ++(history_gm_record_count);
    }
    else
    {
        if (gm_hist_record_file)
        {
            fclose(gm_hist_record_file);
            gm_hist_record_file = nullptr;
        }
        history_gm_record_count = 0;
    }
}

void CollisionDetection::LowPassFilter::reset()
{
    last_x = 0;
    last_y = 0;
}

double CollisionDetection::LowPassFilter::do_filter(double cutFrq, double input)
{
    double k = 0;
    k = 1.0 / 2.0 / M_PI / cutFrq;
    double k1 = 1.0 / (1.0 + 2 * k / 0.008);
    double k2 = -(1.0 - 2 * k / 0.008) / (1.0 + 2 * k / 0.008);
    double output = k1 * (input + last_x) + k2 * last_y;
    //update
    last_y = output;
    last_x = input;
    return output;
}
